import numpy as np
from math import sqrt


def find_farthest_point_from_segment_function(local_path_x, local_path_y):
    """
    找到路径点中距离第一个点与最后一个点连线段D最远的点以及对应的距离值

    参数:
    local_path_x: 路径点的x坐标列表
    local_path_y: 路径点的y坐标列表

    返回:
    farthest_point: 最远点的坐标 (x, y)
    farthest_distance: 最远距离值
    farthest_index: 最远点在列表中的索引
    """

    if len(local_path_x) < 2 or len(local_path_y) < 2:
        raise ValueError("路径点数量不足，至少需要2个点")

    if len(local_path_x) != len(local_path_y):
        raise ValueError("x坐标和y坐标列表长度不一致")

    # 线段D的两个端点
    point_a = np.array([local_path_x[0], local_path_y[0]])
    point_b = np.array([local_path_x[-1], local_path_y[-1]])

    # 线段向量和长度
    segment_vector = point_b - point_a
    segment_length_squared = np.dot(segment_vector, segment_vector)

    # 如果线段长度为0（两点重合），则计算所有点到该点的距离
    if segment_length_squared < 1e-10:
        return find_farthest_point_from_point(point_a, local_path_x, local_path_y)

    max_distance = -1.0
    farthest_point = (local_path_x[0], local_path_y[0])
    farthest_index = 0

    # 遍历所有路径点，计算到线段D的距离
    for i in range(len(local_path_x)):
        current_point = np.array([local_path_x[i], local_path_y[i]])
        distance = point_to_segment_distance(current_point, point_a, point_b, segment_vector, segment_length_squared)

        if distance > max_distance:
            max_distance = distance
            farthest_point = (local_path_x[i], local_path_y[i])
            farthest_index = i

    return farthest_point, max_distance, farthest_index


def point_to_segment_distance(point, segment_start, segment_end, segment_vector, segment_length_squared):
    """
    计算点到线段的距离

    参数:
    point: 目标点坐标
    segment_start: 线段起点
    segment_end: 线段终点
    segment_vector: 线段向量
    segment_length_squared: 线段长度的平方

    返回:
    点到线段的距离
    """
    # 计算投影参数t
    vector_to_point = point - segment_start
    t = np.dot(vector_to_point, segment_vector) / segment_length_squared

    if t < 0:
        # 点在线段起点外侧，计算到起点的距离
        return np.linalg.norm(point - segment_start)
    elif t > 1:
        # 点在线段终点外侧，计算到终点的距离
        return np.linalg.norm(point - segment_end)
    else:
        # 点在线段范围内，计算垂直距离
        projection_point = segment_start + t * segment_vector
        return np.linalg.norm(point - projection_point)


def find_farthest_point_from_point(reference_point, local_path_x, local_path_y):
    """
    当线段长度为0时，计算所有点到参考点的距离并找到最远点
    """
    max_distance = -1.0
    farthest_point = (local_path_x[0], local_path_y[0])
    farthest_index = 0

    for i in range(len(local_path_x)):
        current_point = np.array([local_path_x[i], local_path_y[i]])
        distance = np.linalg.norm(current_point - reference_point)

        if distance > max_distance:
            max_distance = distance
            farthest_point = (local_path_x[i], local_path_y[i])
            farthest_index = i

    return farthest_point, max_distance, farthest_index


def calculate_segment_equation(local_path_x, local_path_y):
    """
    计算线段D的直线方程: Ax + By + C = 0
    """
    x1, y1 = local_path_x[0], local_path_y[0]
    x2, y2 = local_path_x[-1], local_path_y[-1]

    if abs(x2 - x1) < 1e-10:  # 垂直线
        A = 1
        B = 0
        C = -x1
    else:
        # 一般式: (y2-y1)x - (x2-x1)y + (x2*y1 - x1*y2) = 0
        A = y2 - y1
        B = -(x2 - x1)
        C = x2 * y1 - x1 * y2

    # 归一化系数
    norm = sqrt(A ** 2 + B ** 2)
    if norm > 1e-10:
        A /= norm
        B /= norm
        C /= norm

    return A, B, C


# 测试函数
def test_farthest_point_finder():
    """
    测试函数
    """
    # 测试用例1: 普通情况
    print("测试用例1: 普通路径")
    local_path_x = [0, 1, 2, 3, 4, 5]
    local_path_y = [0, 2, 1, 3, 2, 0]

    farthest_point, farthest_distance, farthest_index = find_farthest_point_from_segment(local_path_x, local_path_y)

    print(f"线段D: ({local_path_x[0]}, {local_path_y[0]}) -> ({local_path_x[-1]}, {local_path_y[-1]})")
    print(f"最远点: ({farthest_point[0]:.2f}, {farthest_point[1]:.2f})")
    print(f"最远距离: {farthest_distance:.2f}")
    print(f"最远点索引: {farthest_index}")

    # 计算直线方程
    A, B, C = calculate_segment_equation(local_path_x, local_path_y)
    print(f"直线方程: {A:.2f}x + {B:.2f}y + {C:.2f} = 0")

    # 测试用例2: 两点重合的情况
    print("\n测试用例2: 两点重合")
    local_path_x2 = [2, 3, 4, 5, 6, 2]
    local_path_y2 = [2, 4, 3, 5, 4, 2]

    farthest_point2, farthest_distance2, farthest_index2 = find_farthest_point_from_segment(local_path_x2,
                                                                                            local_path_y2)
    print(f"最远点: ({farthest_point2[0]:.2f}, {farthest_point2[1]:.2f})")
    print(f"最远距离: {farthest_distance2:.2f}")

    # 测试用例3: 水平线段
    print("\n测试用例3: 水平线段")
    local_path_x3 = [0, 1, 2, 3, 4, 5]
    local_path_y3 = [0, 1, 3, 2, 1, 0]

    farthest_point3, farthest_distance3, farthest_index3 = find_farthest_point_from_segment(local_path_x3,
                                                                                            local_path_y3)
    print(f"最远点: ({farthest_point3[0]:.2f}, {farthest_point3[1]:.2f})")
    print(f"最远距离: {farthest_distance3:.2f}")

    # 可视化
    try:
        import matplotlib.pyplot as plt

        plt.figure(figsize=(12, 4))

        # 子图1: 测试用例1
        plt.subplot(1, 3, 1)
        plt.plot(local_path_x, local_path_y, 'bo-', label='路径点')
        plt.plot([local_path_x[0], local_path_x[-1]], [local_path_y[0], local_path_y[-1]],
                 'r--', linewidth=2, label='线段D')
        plt.plot(farthest_point[0], farthest_point[1], 'm*', markersize=15,
                 label=f'最远点 (距离={farthest_distance:.2f})')
        plt.grid(True)
        plt.legend()
        plt.title('测试用例1')
        plt.axis('equal')

        # 子图2: 测试用例2
        plt.subplot(1, 3, 2)
        plt.plot(local_path_x2, local_path_y2, 'bo-', label='路径点')
        plt.plot([local_path_x2[0], local_path_x2[-1]], [local_path_y2[0], local_path_y2[-1]],
                 'r--', linewidth=2, label='线段D')
        plt.plot(farthest_point2[0], farthest_point2[1], 'm*', markersize=15,
                 label=f'最远点 (距离={farthest_distance2:.2f})')
        plt.grid(True)
        plt.legend()
        plt.title('测试用例2 (两点重合)')
        plt.axis('equal')

        # 子图3: 测试用例3
        plt.subplot(1, 3, 3)
        plt.plot(local_path_x3, local_path_y3, 'bo-', label='路径点')
        plt.plot([local_path_x3[0], local_path_x3[-1]], [local_path_y3[0], local_path_y3[-1]],
                 'r--', linewidth=2, label='线段D')
        plt.plot(farthest_point3[0], farthest_point3[1], 'm*', markersize=15,
                 label=f'最远点 (距离={farthest_distance3:.2f})')
        plt.grid(True)
        plt.legend()
        plt.title('测试用例3 (水平线段)')
        plt.axis('equal')

        plt.tight_layout()
        plt.show()

    except ImportError:
        print("未安装matplotlib，跳过可视化")


# 使用示例
if __name__ == "__main__":
    # 示例使用
    local_path_x = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
    local_path_y = [0, 1, 3, 2, 4, 3, 2, 1, 0, -1, 0]

    farthest_point, farthest_distance, farthest_index = find_farthest_point_from_segment(local_path_x, local_path_y)

    print(f"线段D: 点A({local_path_x[0]}, {local_path_y[0]}) -> 点B({local_path_x[-1]}, {local_path_y[-1]})")
    print(f"最远点C: ({farthest_point[0]:.2f}, {farthest_point[1]:.2f})")
    print(f"最远距离d: {farthest_distance:.2f}")
    print(f"最远点索引: {farthest_index}")

    # 运行测试
    test_farthest_point_finder()